#include "pid.h"




// 初始化PI控制器
void IncrementalPI_Init(IncrementalPIController *pi, uint8_t Kp, uint8_t Ki)
{
    pi->Kp = Kp;
    pi->Ki = Ki;
    pi->Kd = 0;
    pi->prev_error = 0;
    pi->integral = 0;
    pi->scaling = 4;    // 初始缩放因子为1，不缩放

    // printf("IncrementalPI pi = %d Kp = %d Ki = %d \r\n", pi, pi->Kp, pi->Ki );
}

void IncrementalPID_Init(IncrementalPIController *pi, uint8_t Kp, uint8_t Ki, uint8_t Kd)
{
    pi->Kp = Kp;
    pi->Ki = Ki;
    pi->Kd = Kd;
    pi->prev_error = 0;
    pi->integral = 0;
    pi->scaling = 32;    // 初始缩放因子为1，不缩放

    // printf("IncrementalPI pi = %d Kp = %d Ki = %d \r\n", pi, pi->Kp, pi->Ki );
}

// 计算增量式PI输出（定点数运算）
uint8_t IncrementalPI_Compute(IncrementalPIController *pi, uint16_t setpoint, uint16_t measured_value, uint8_t max_output)
{
    // 计算当前误差（定点数）
    int16_t error = (setpoint - measured_value) ;

    // 计算误差的变化量（差分）
    int16_t error_diff = error - pi->prev_error;

    if(error_diff > 200)
    {
        error_diff = 200;
    }
    else
    if(error_diff < -200)
    {
        error_diff = -200;
    }
    // 计算积分增量（注意：这里我们直接累加积分，但使用定点数）
    pi->integral += error;
    if(pi->integral > 200)
    {
        pi->integral = 200;
    }
    else if(pi->integral < -200)
    {
        pi->integral = -200;
    }

    // 为了避免整数溢出，我们可以在这里检查并限制integral的值
    // 但在这个简单示例中，我们省略了这个步骤

    // 计算控制量的增量（定点数）
    int32_t delta_output; 
    // delta_output = (pi->Kp * error_diff + pi->Ki * pi->integral) / pi->scaling;     // 增量式PI控制器
    delta_output = (pi->Kp * error + pi->Ki * pi->integral + pi->Kd * error_diff) / pi->scaling;     // 位置式PID控制器
    #if DEBUG_PRINTF
    printf("PID: p = %d i = %d d = %d\r\n",pi->Kp, pi->Ki, pi->Kd);
    printf("error = %d error_diff = %d pi->integral = %d delta_output = %d\r\n",error, error_diff, pi->integral, delta_output);
    #endif
    if(delta_output < 0)
    {
        delta_output = 0;
    }
    else
    if(delta_output > max_output)
    {
        delta_output = max_output;
    }

    // 更新状态变量
    pi->prev_error = error;

    // 返回控制量的增量（注意：这里返回的是定点数，需要在实际应用时根据需要进行缩放）
    return (uint8_t)(delta_output); // 假设我们想要16位定点数输出，右移以缩放
}

